#include "Matrix3f.h"
#include "SharpShooterMath.h"

//! sharpShooter
namespace sharpShooter
{	
	//! Core
	namespace Core
	{
		const float Matrix3f::EPSILON = (float)(1e-06);
		const Matrix3f Matrix3f::ZERO(0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f);
		const Matrix3f Matrix3f::IDENTITY(1.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,1.0f);


		//bool Matrix3f::operator ==(Matrix3f &mat)
		//{
		//	for (int i=0; i<3; i++)
		//	{
		//		for (int j=0; j<3; j++)
		//		{
		//			if (!MathLib::floatEqual(mElement[i][j],mat.mElement[i][j]))
		//			{
		//				return false;
		//			}
		//		}
		//	}
		//	return true;
		//}

		//Matrix3f Matrix3f::operator +(const Matrix3f &mat)
		//{
		//	Matrix3f matAdd;
		//	for (int row=0; row<3; row++)
		//	{
		//		for (int col=0; col<3; col++)
		//		{
		//			matAdd.mElement[row][col]=mElement[row][col] + mat.mElement[row][col];
		//		}
		//	}
		//	return matAdd;
		//}

		//Matrix3f Matrix3f::operator -(const Matrix3f &mat)
		//{
		//	Matrix3f matSub;
		//	for (int row=0; row<3; row++)
		//	{
		//		for (int col=0; col<3; col++)
		//		{
		//			matSub.mElement[row][col]=mElement[row][col] - mat.mElement[row][col];
		//		}
		//	}
		//	return matSub;
		//}

		//Matrix3f Matrix3f::operator *(const Matrix3f &mat)
		//{
		//	Matrix3f product;
		//	for (int row=0; row<3; row++)
		//	{
		//		for (int col=0; col<3; col++)
		//		{
		//			product.mElement[row][col] = mElement[row][0]*mat.mElement[0][col] + mElement[row][1]*mat.mElement[1][col] + mElement[row][2]*mat.mElement[2][col]; 
		//		}
		//	}
		//	return product;
		//}

		//void Matrix3f::transformVector3f(Vector3f &vec)
		//{
		//	float resultVector[3];
		//	resultVector[0] = vec.mX*mElement[0][0] + vec.mY*mElement[1][0] + vec.mZ*mElement[2][0];
		//	resultVector[1] = vec.mX*mElement[0][1] + vec.mY*mElement[1][1] + vec.mZ*mElement[2][1];
		//	resultVector[2] = vec.mX*mElement[0][2] + vec.mY*mElement[1][2] + vec.mZ*mElement[2][2];

		//	vec.mX = resultVector[0];
		//	vec.mY = resultVector[1];
		//	vec.mZ = resultVector[2];
		//}
		//void Matrix3f::transformVector3f(Vector3f &out,const Vector3f &in)
		//{
		//	out.mX = in.mX*mElement[0][0] + in.mY*mElement[1][0] + in.mZ*mElement[2][0];
		//	out.mY = in.mX*mElement[0][1] + in.mY*mElement[1][1] + in.mZ*mElement[2][1];
		//	out.mZ = in.mX*mElement[0][2] + in.mY*mElement[1][2] + in.mZ*mElement[2][2];
		//}
		//Matrix3f Matrix3f::operator *(const float fScalar)
		//{
		//	Matrix3f mat;
		//	for (int row=0; row<3; row++)
		//	{
		//		for (int col=0; col<3; col++)
		//		{
		//			mat.mElement[row][col] = mElement[row][col]*fScalar;
		//		}
		//	}
		//	return mat;
		//}

		//Matrix3f Matrix3f::Transpose() const
		//{
		//	Matrix3f mat;
		//	for (size_t row =0; row<3; row++)
		//	{
		//		for (size_t col=0; col<3; col++)
		//		{
		//			mat.mElement[row][col] = mElement[col][row];
		//		}
		//	}
		//	return mat;
		//}

		//bool Matrix3f::Inverse(Matrix3f &inverse, float fTolerance)
		//{
		//	inverse.mElement[0][0] = mElement[1][1]*mElement[2][2] - mElement[1][2]*mElement[2][1];
		//	inverse.mElement[0][1] = mElement[0][2]*mElement[2][1] - mElement[0][1]*mElement[2][2];
		//	inverse.mElement[0][2] = mElement[0][1]*mElement[1][2] - mElement[0][2]*mElement[1][1];
		//	inverse.mElement[1][0] = mElement[1][2]*mElement[2][0] - mElement[1][0]*mElement[2][2];
		//	inverse.mElement[1][1] = mElement[0][0]*mElement[2][2] - mElement[0][2]*mElement[2][0];
		//	inverse.mElement[1][2] = mElement[0][2]*mElement[1][0] - mElement[0][0]*mElement[1][2];
		//	inverse.mElement[2][0] = mElement[1][0]*mElement[2][1] - mElement[1][1]*mElement[2][0];
		//	inverse.mElement[2][1] = mElement[0][1]*mElement[2][0] - mElement[0][0]*mElement[2][1];
		//	inverse.mElement[2][2] = mElement[0][0]*mElement[1][1] - mElement[0][1]*mElement[1][0];

		//	float fDet = mElement[0][0]*inverse.mElement[0][0]
		//	+ mElement[0][1]*inverse.mElement[1][0]
		//	+ mElement[0][2]*inverse.mElement[2][0];
		//	if (abs(fDet)<fTolerance)
		//	{
		//		return false;
		//	}

		//	for (int row=0; row<3; row++)
		//	{
		//		for (int col=0; col<3; col++)
		//		{
		//			inverse.mElement[row][col] /= fDet;
		//		}
		//	}
		//	return true;
		//}
		/*void Matrix3f::setRotation(const Vector3f &rotation)
		{
			double cr = cos( rotation.mX );
			double sr = sin( rotation.mX );
			double cp = cos( rotation.mY );
			double sp = sin( rotation.mY );
			double cy = cos( rotation.mZ );
			double sy = sin( rotation.mZ );

			mElement[0][0] = (float)(cp*cy);
			mElement[0][1] = (float)(cp*sy);
			mElement[0][2] = (float)(-sp);

			double srsp = (float)(sr*sp);
			double crsp = (float)(cr*sp);

			mElement[1][0] = (float)(srsp*cy-cr*sy);
			mElement[1][1] = (float)(srsp*sy+cr*cy);
			mElement[1][2] = (float)(sr*cp);

			mElement[2][0] = (float)(crsp*cy+sr*sy);
			mElement[2][1] = (float)(crsp*sy-sr*cy);
			mElement[2][2] = (float)(cr*cp);
		}*/
		//void Matrix3f::toAxisAngle(Vector3f &vAxis, float &fAngle)
		//{
		//	// Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
		//	// The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
		//	// I is the identity and
		//	//
		//	//       +-        -+
		//	//   P = |  0 -z +y |
		//	//       | +z  0 -x |
		//	//       | -y +x  0 |
		//	//       +-        -+
		//	//
		//	// If A > 0, R represents a counterclockwise rotation about the axis in
		//	// the sense of looking from the tip of the axis vector towards the
		//	// origin.  Some algebra will show that
		//	//
		//	//   cos(A) = (trace(R)-1)/2  and  R - R^t = 2*sin(A)*P
		//	//
		//	// In the event that A = pi, R-R^t = 0 which prevents us from extracting
		//	// the axis through P.  Instead note that R = I+2*P^2 when A = pi, so
		//	// P^2 = (R-I)/2.  The diagonal entries of P^2 are x^2-1, y^2-1, and
		//	// z^2-1.  We can solve these for axis (x,y,z).  Because the angle is pi,
		//	// it does not matter which sign you choose on the square roots.
		//
		//	float fTrace = mElement[0][0]+mElement[1][1]+mElement[2][2];
		//	fAngle = acosf(0.5f * (fTrace - 1.0f));
		//	if (fAngle>0)
		//	{
		//		if (fAngle<Math::PI)
		//		{
		//			vAxis.mX = mElement[2][1] - mElement[1][2];
		//			vAxis.mY = mElement[2][0] - mElement[0][2];
		//			vAxis.mZ = mElement[1][0] - mElement[0][1];
		//			vAxis.normalize();
		//		}
		//		else
		//		{
		//			float fHarfInverse;
		//			if (mElement[0][0]>mElement[1][1])
		//			{
		//				if (mElement[0][0]>mElement[2][2])
		//				{
		//					vAxis.mX = 0.5f * sqrt(mElement[0][0] - mElement[1][1] - mElement[2][2] + 1.0f);
		//					fHarfInverse = 0.5f/vAxis.mX;
		//					vAxis.mY = fHarfInverse * mElement[0][1];
		//					vAxis.mZ = fHarfInverse * mElement[0][2];
		//				}
		//				else
		//				{
		//					vAxis.mZ = 0.5f * sqrt(mElement[2][2] - mElement[0][0] - mElement[1][1] + 1.0f);
		//					fHarfInverse = 0.5f/vAxis.mZ;
		//					vAxis.mY = fHarfInverse * mElement[1][2];
		//					vAxis.mX = fHarfInverse * mElement[0][2];
		//				}
		//			}
		//			else
		//			{
		//				if (mElement[1][1]>mElement[2][2])
		//				{
		//					vAxis.mY = 0.5f * sqrt(mElement[1][1] - mElement[0][0] - mElement[2][2] + 1.0f);
		//					fHarfInverse = 0.5f/vAxis.mY;
		//					vAxis.mX = fHarfInverse * mElement[0][1];
		//					vAxis.mZ = fHarfInverse * mElement[1][2];
		//				}
		//				else
		//				{
		//					vAxis.mZ = 0.5f * sqrt(mElement[2][2] - mElement[0][0] - mElement[1][1] + 1.0f);
		//					fHarfInverse = 0.5f/vAxis.mZ;
		//					vAxis.mY = fHarfInverse * mElement[1][2];
		//					vAxis.mX = fHarfInverse * mElement[0][2];
		//				}
		//			}
		//		}
		//	}
		//
		//	else
		//	{
		//		vAxis.mX = 1.0f;
		//		vAxis.mY = 0.0f;
		//		vAxis.mZ = 0.0f;
		//	}
		//}
		//
		//void Matrix3f::fromAxisAngle(const Vector3f &vAxis, const float &fAngle)
		//{
		//	// left handed coordinate system:
		//	//  cos = cos(fAngle) sin = sin(fAngle)   X=vAxis.mX   Y=vAxis.mY   Z=vAxis.mZ
		//	//				| cos+X^2(1-cos)              XY(1-cos)+Zsin                XZ(1-cos)-Ysin|
		//	//				| XY(1-cos)-Zsin			  cos + Y^2(1-cos)				YZ(1-cos)+Xsin|
		//	//				| XZ(1-cos)+Ysin			  YZ(1-cos)-Xsin				cos+Z^2(1-cos)|

		//	float fCos = cos(fAngle);
		//	float fSin = sin(fAngle);
		//	float f1MCos = 1-fCos;
		//	float fXYC = vAxis.mX*vAxis.mY*f1MCos;
		//	float fYZC = vAxis.mY*vAxis.mZ*f1MCos;
		//	float fXZC = vAxis.mX*vAxis.mZ*f1MCos;
		//	mElement[0][0]=fCos+vAxis.mX*vAxis.mX*f1MCos;
		//	mElement[0][1]=fXYC+vAxis.mZ*fSin;
		//	mElement[0][2]=fXZC-vAxis.mY*fSin;
		//	mElement[1][0]=fXYC-vAxis.mZ*fSin;
		//	mElement[1][1]=fCos+vAxis.mY*vAxis.mY*f1MCos;
		//	mElement[1][2]=fYZC+vAxis.mX*fSin;
		//	mElement[2][0]=fXZC+vAxis.mY*fSin;
		//	mElement[2][1]=fXZC-vAxis.mX*fSin;
		//	mElement[2][2]=fCos+vAxis.mZ*vAxis.mZ*f1MCos;
		//}
		//void Matrix3f::fromEulerAngleXYZ(const float& fYaw, const float& fPitch, const float& fRoll)
		//{
		//	float fCos,fSin;
		//
		//	fCos = cos(fYaw); 
		//	fSin = sin(fYaw);
		//	Matrix3f Mx(1.0f,0.0f,0.0f,0.0f,fCos,-fSin,0.0f,fSin,fCos);
		//
		//	fCos = cos(fPitch);
		//	fSin = sin(fPitch);
		//	Matrix3f My(fCos,0.0f,fSin,0.0f,1.0f,0.0f,-fSin,0.0f,fCos);
		//
		//	fCos = cos(fRoll);
		//	fSin = sin(fRoll);
		//	Matrix3f Mz(fCos,-fSin,0.0f,fSin,fCos,0.0f,0.0f,0.0f,1.0f);
		//
		//	operator=(Mx*(My*Mz));
		//}
		//void Matrix3f::fromEulerAngleXZY(const float &fYaw, const float &fPitch, const float &fRoll)
		//{
		//	float fCos,fSin;
		//
		//	fCos = cos(fYaw); 
		//	fSin = sin(fYaw);
		//	Matrix3f Mx(1.0f,0.0f,0.0f,0.0f,fCos,-fSin,0.0f,fSin,fCos);
		//
		//	fCos = cos(fRoll);
		//	fSin = sin(fRoll);
		//	Matrix3f Mz(fCos,-fSin,0.0f,fSin,fCos,0.0f,0.0f,0.0f,1.0f);
		//
		//	fCos = cos(fPitch);
		//	fSin = sin(fPitch);
		//	Matrix3f My(fCos,0.0f,fSin,0.0f,1.0f,0.0f,-fSin,0.0f,fCos);
		//
		//	operator=(Mx*(Mz*My));
		//}
		//void Matrix3f::fromEulerAngleYXZ(const float &fYaw, const float &fPitch, const float &fRoll)
		//{
		//	float fCos,fSin;
		//
		//	fCos = cos(fPitch);
		//	fSin = sin(fPitch);
		//	Matrix3f My(fCos,0.0f,fSin,0.0f,1.0f,0.0f,-fSin,0.0f,fCos);
		//
		//	fCos = cos(fYaw); 
		//	fSin = sin(fYaw);
		//	Matrix3f Mx(1.0f,0.0f,0.0f,0.0f,fCos,-fSin,0.0f,fSin,fCos);
		//
		//	fCos = cos(fRoll);
		//	fSin = sin(fRoll);
		//	Matrix3f Mz(fCos,-fSin,0.0f,fSin,fCos,0.0f,0.0f,0.0f,1.0f);
		//
		//	operator=(My*(Mx*Mz));
		//}
		//
		//
		//void Matrix3f::fromEulerAngleYZX(const float &fYaw, const float &fPitch, const float &fRoll)
		//{
		//	float fCos,fSin;
		//
		//	fCos = cos(fPitch);
		//	fSin = sin(fPitch);
		//	Matrix3f My(fCos,0.0f,fSin,0.0f,1.0f,0.0f,-fSin,0.0f,fCos);
		//
		//	fCos = cos(fRoll);
		//	fSin = sin(fRoll);
		//	Matrix3f Mz(fCos,-fSin,0.0f,fSin,fCos,0.0f,0.0f,0.0f,1.0f);
		//
		//	fCos = cos(fYaw); 
		//	fSin = sin(fYaw);
		//	Matrix3f Mx(1.0f,0.0f,0.0f,0.0f,fCos,-fSin,0.0f,fSin,fCos);
		//
		//	operator=(My*(Mz*Mx));
		//}
		//
		//void Matrix3f::fromEulerAngleZXY(const float &fYaw, const float &fPitch, const float &fRoll)
		//{
		//	float fCos,fSin;
		//
		//	fCos = cos(fRoll);
		//	fSin = sin(fRoll);
		//	Matrix3f Mz(fCos,-fSin,0.0f,fSin,fCos,0.0f,0.0f,0.0f,1.0f);
		//
		//	fCos = cos(fYaw); 
		//	fSin = sin(fYaw);
		//	Matrix3f Mx(1.0f,0.0f,0.0f,0.0f,fCos,-fSin,0.0f,fSin,fCos);
		//
		//	fCos = cos(fPitch);
		//	fSin = sin(fPitch);
		//	Matrix3f My(fCos,0.0f,fSin,0.0f,1.0f,0.0f,-fSin,0.0f,fCos);
		//
		//	operator=(Mz*(Mx*My));
		//}
		//
		//void Matrix3f::fromEulerAngleZYX(const float &fYaw, const float &fPitch, const float &fRoll)
		//{
		//	float fCos,fSin;
		//
		//	fCos = cos(fRoll);
		//	fSin = sin(fRoll);
		//	Matrix3f Mz(fCos,-fSin,0.0f,fSin,fCos,0.0f,0.0f,0.0f,1.0f);
		//
		//	fCos = cos(fPitch);
		//	fSin = sin(fPitch);
		//	Matrix3f My(fCos,0.0f,fSin,0.0f,1.0f,0.0f,-fSin,0.0f,fCos);
		//
		//	fCos = cos(fYaw); 
		//	fSin = sin(fYaw);
		//	Matrix3f Mx(1.0f,0.0f,0.0f,0.0f,fCos,-fSin,0.0f,fSin,fCos);
		//
		//	operator=(Mz*(My*Mx));
		//}





	}
}